/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "robotMath.h"
#include <math.h>


//calculates max circle radius for a differential drive robot
//at a desired translational velocity and wheel speed limitation

float CircleRadiusFromVelocities(float v, float wheel_speed_limit, float axle_width) {
    float w = 2.0*(v-wheel_speed_limit)/axle_width;
    if (fabs(w)<0.00001)
        return 10000.0;
    else
        return (v/fabs(w));    
}

float NormalizeAngleRadians(float heading) {
	float ang_temp = heading;	
	while (fabs(ang_temp)>3.14159265)	{
		if (ang_temp >0.0) 
			ang_temp = ang_temp-2.0*3.14159265;	
		else
			ang_temp = ang_temp+2.0*3.14159265;
	}
	return ang_temp;
}

float NormalizeAngleDegrees(float heading) {
	float ang_temp = heading;	
	while (fabs(ang_temp)>180.0)	{
		if (ang_temp >0.0) 
			ang_temp = ang_temp-2.0*180.0;	
		else
			ang_temp = ang_temp+2.0*180.0;
	}
	return ang_temp;
}

float GetAngleBetweenVectors(float v1_mag, float v1_ang, float v2_mag, float v2_ang) {
	float x_comp = cos(v1_ang)*v1_mag + cos(v2_ang)*v2_mag;
	float y_comp = sin(v1_ang)*v1_mag + sin(v2_ang)*v2_mag;

	return atan2(y_comp,x_comp);
}

float ConvertMinus180Plus180ToZero360 (float angle) {
    
    float temp = NormalizeAngleDegrees(angle);
    
    if (temp>0.0)		
	return temp;
    else
	return (180.0 + 180.0 + temp); 
    
}



